#include <jni.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <vector>
#include "opencv2/highgui/highgui.hpp"
#include <android/log.h>


#include <math.h>
#include <string.h>


using namespace std;
using namespace cv;

extern "C" {
// t1 I reduced the number N for thresholds
// in t2 I tried to use the original image only for calculation and not to copy
// in t3 I tried to remove useless mixxing channels
// in t4 use small image to calculate

int thresh = 50, N = 1;
const char* wndname = "Square Detection Demo";
char buffer[100];

// helper function:
// finds a cosine of angle between vectors
// from pt0->pt1 and from pt0->pt2
static double angle( Point pt1, Point pt2, Point pt0 )
{
    double dx1 = pt1.x - pt0.x;
    double dy1 = pt1.y - pt0.y;
    double dx2 = pt2.x - pt0.x;
    double dy2 = pt2.y - pt0.y;
    return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

// returns sequence of squares detected on the image.
// the sequence is stored in the specified memory storage
static void findSquares( const Mat& image, vector<vector<Point> >& squares )
{
    squares.clear();

    Mat pyr, timg, gray0(image.size(), CV_8U), gray;

    // down-scale and upscale the image to filter out the noise
    pyrDown(image, pyr, Size(image.cols/2, image.rows/2));
    pyrUp(pyr, timg, image.size()); // commented in t4
    
    vector<vector<Point> > contours;

    // find squares in every color plane of the image
   // for( int c = 0; c < 3; c++ )
   for( int c = 0; c < 1; c++ )
    {
        int ch[] = {c, 0};
  //      mixChannels(&timg, 1, &gray0, 1, ch, 1); t3

        // try several threshold levels
        for( int l = 0; l < N; l++ )
        {
            // hack: use Canny instead of zero threshold level.
            // Canny helps to catch squares with gradient shading
            if( l == 0 )
            {
                // apply Canny. Take the upper threshold from slider
                // and set the lower to 0 (which forces edges merging)
                //Canny(gray0, gray, 0, thresh, 5);//org
               // Canny(timg, gray, 0, thresh, 5);//t3
               Canny(timg, gray, 50, 200, 5); // change the lower and upper limits
                //Canny(pyr, gray, 0, thresh, 5);//t4
                // dilate canny output to remove potential
                // holes between edge segments
                dilate(gray, gray, Mat(), Point(-1,-1));
            }
            else
            {
                // apply threshold if l!=0:
                //     tgray(x,y) = gray(x,y) < (l+1)*255/N ? 255 : 0
                //gray = gray0 >= (l+1)*255/N;//org
                gray = timg >= (l+1)*255/N;//t3
              //Canny(pyr, gray, 0, thresh, 5);//t4
            }

            // find contours and store them all as a list
            findContours(gray, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

            vector<Point> approx;

            // test each contour
            for( size_t i = 0; i < contours.size(); i++ )
            {
                // approximate contour with accuracy proportional
                // to the contour perimeter
                approxPolyDP(Mat(contours[i]), approx, arcLength(Mat(contours[i]), true)*0.02, true);

                // square contours should have 4 vertices after approximation
                // relatively large area (to filter out noisy contours)
                // and be convex.
                // Note: absolute value of an area is used because
                // area may be positive or negative - in accordance with the
                // contour orientation
                if( approx.size() == 4 &&
                    fabs(contourArea(Mat(approx))) > 1000 &&
                    isContourConvex(Mat(approx)) )
                {
                    double maxCosine = 0;

                    for( int j = 2; j < 5; j++ )
                    {
                        // find the maximum cosine of the angle between joint edges
                        double cosine = fabs(angle(approx[j%4], approx[j-2], approx[j-1]));
                        maxCosine = MAX(maxCosine, cosine);
                    }

                    // if cosines of all angles are small
                    // (all angles are ~90 degree) then write quandrange
                    // vertices to resultant sequence
                    if( maxCosine < 0.3 )
                        squares.push_back(approx);
                }
            }
        }
    }
}

// the function draws all the squares in the image
static void drawSquares( Mat& image, const vector<vector<Point> >& squares )
{

    for( size_t i = 0; i < squares.size(); i++ )
    {
        const Point* p = &squares[i][0];
        int n = (int)squares[i].size();
        sprintf(buffer, "test int = %d", n);
	__android_log_print(ANDROID_LOG_INFO, "sometag", buffer);
        polylines(image, &p, &n, 1, true, Scalar(0,255,0,255), 3, CV_AA);
    }

   
}

JNIEXPORT void JNICALL Java_org_opencv_samples_tutorial4_Sample4View_FindFeatures2(JNIEnv* env, jobject, jint width, jint height, jbyteArray yuv, jintArray bgra)
{
    jbyte* _yuv  = env->GetByteArrayElements(yuv, 0);
    jint*  _bgra = env->GetIntArrayElements(bgra, 0);
    

    Mat myuv(height + height/2, width, CV_8UC1, (unsigned char *)_yuv);
    Mat mbgra(height, width, CV_8UC4, (unsigned char *)_bgra);
    //Mat mbgr(height, width, CV_8UC3, (unsigned char *)_bgra);
    Mat mgray(height, width, CV_8UC1, (unsigned char *)_yuv);
    //Mat mgray(height, width, CV_8UC1, (unsigned char *)_bgra);//t2

    //Please make attention about BGRA byte order
    //ARGB stored in java as int array becomes BGRA at native level
    cvtColor(myuv, mbgra, CV_YUV420sp2BGR, 4);
    cvtColor(myuv, mgray, CV_YUV420sp2GRAY, 1);
    //cvtColor(mbgra, mgray, CV_BGR2GRAY, 1);
    //cvtColor(mgray, mbgra, CV_GRAY2BGR, 4); // commented in t2

    vector<vector<Point> > squares;	

   findSquares(mgray, squares);
   drawSquares(mbgra, squares);
   //drawSquares(myuv, squares);//t2

    env->ReleaseIntArrayElements(bgra, _bgra, 0);
    //env->ReleaseIntArrayElements(bgra, _yuv, 0);// t2
    env->ReleaseByteArrayElements(yuv, _yuv, 0);
}

JNIEXPORT void JNICALL Java_org_opencv_samples_tutorial4_Sample4View_FindFeatures(JNIEnv*, jobject, jlong addrGray, jlong addrRgba)
{
    Mat* pMatGr=(Mat*)addrGray;
    Mat* pMatRgb=(Mat*)addrRgba;
    //vector<KeyPoint> v;
    vector<vector<Point> > squares;	

   findSquares(*pMatGr, squares);
   drawSquares(*pMatRgb, squares);
   

}

}
